
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_follow.c
  * @author     baiyang
  * @date       2023-8-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mb_follow.h"

#include <ahrs/ahrs_view.h>
#include <parameter/param.h>
#include <common/time/gp_time.h>
#include <vehicle/vehicle_type.h>
/*-----------------------------------macro------------------------------------*/
#define MB_FOLLOW_TIMEOUT_MS    3000    // position estimate timeout after 1 second
#define MB_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we haave not heard from them in 10 seconds

#define MB_FOLLOW_OFFSET_TYPE_NED       0   // offsets are in north-east-down frame
#define MB_FOLLOW_OFFSET_TYPE_RELATIVE  1   // offsets are relative to lead vehicle's heading

#define MB_FOLLOW_ALTITUDE_TYPE_RELATIVE  1 // relative altitude is used by default   

#define MB_FOLLOW_POS_P_DEFAULT 0.1f    // position error gain default

#if MB_BUILD_TYPE(MICROBEE_BUILD_Plane)
#define MB_FOLLOW_ALT_TYPE_DEFAULT 0
#else
#define MB_FOLLOW_ALT_TYPE_DEFAULT MB_FOLLOW_ALTITUDE_TYPE_RELATIVE
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void mb_follow_assign_param();
/*----------------------------------variable----------------------------------*/
/* define parameters */
static param_t var_info[] = {
    // @Param: _ENABLE
    // @DisplayName: Follow enable/disable
    // @Description: Enabled/disable following a target
    // @Values: 0:Disabled,1:Enabled
    // @User: Standard
    PARAM_DEFINE_INT8(FOLL_ENABLE, 0),

    // @Param: _SYSID
    // @DisplayName: Follow target's mavlink system id
    // @Description: Follow target's mavlink system id
    // @Range: 0 255
    // @User: Standard
    PARAM_DEFINE_INT16(FOLL_SYSID, 0),

    // @Param: _DIST_MAX
    // @DisplayName: Follow distance maximum
    // @Description: Follow distance maximum.  targets further than this will be ignored
    // @Units: m
    // @Range: 1 1000
    // @User: Standard
    PARAM_DEFINE_FLOAT(FOLL_DIST_MAX, 100),

    // @Param: _OFS_TYPE
    // @DisplayName: Follow offset type
    // @Description: Follow offset type
    // @Values: 0:North-East-Down, 1:Relative to lead vehicle heading
    // @User: Standard
    PARAM_DEFINE_INT8(FOLL_OFS_TYPE, MB_FOLLOW_OFFSET_TYPE_NED),

    // @Param: _OFS_X
    // @DisplayName: Follow offsets in meters north/forward
    // @Description: Follow offsets in meters north/forward.  If positive, this vehicle fly ahead or north of lead vehicle.  Depends on FOLL_OFS_TYPE
    // @Range: -100 100
    // @Units: m
    // @Increment: 1
    // @User: Standard
    PARAM_DEFINE_FLOAT(FOLL_OFS_X, 0),

    // @Param: _OFS_Y
    // @DisplayName: Follow offsets in meters east/right
    // @Description: Follow offsets in meters east/right.  If positive, this vehicle will fly to the right or east of lead vehicle.  Depends on FOLL_OFS_TYPE
    // @Range: -100 100
    // @Units: m
    // @Increment: 1
    // @User: Standard
    PARAM_DEFINE_FLOAT(FOLL_OFS_Y, 0),

    // @Param: _OFS_Z
    // @DisplayName: Follow offsets in meters down
    // @Description: Follow offsets in meters down.  If positive, this vehicle will fly below the lead vehicle
    // @Range: -100 100
    // @Units: m
    // @Increment: 1
    // @User: Standard
    PARAM_DEFINE_FLOAT(FOLL_OFS_Z, 0),

#if !(MB_BUILD_TYPE(MICROBEE_BUILD_Rover))
    // @Param: _YAW_BEHAVE
    // @DisplayName: Follow yaw behaviour
    // @Description: Follow yaw behaviour
    // @Values: 0:None,1:Face Lead Vehicle,2:Same as Lead vehicle,3:Direction of Flight
    // @User: Standard
    PARAM_DEFINE_INT8(FOLL_YAW_BEHAVE, 1),

    // @Param: _ALT_TYPE
    // @DisplayName: Follow altitude type
    // @Description: Follow altitude type
    // @Values: 0:absolute, 1:relative
    // @User: Standard
    PARAM_DEFINE_INT8(FOLL_ALT_TYPE, MB_FOLLOW_ALT_TYPE_DEFAULT),
#endif

    // @Param: _POS_P
    // @DisplayName: Follow position error P gain
    // @Description: Follow position error P gain.  Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
    // @Range: 0.01 1.00
    // @Increment: 0.01
    // @User: Standard
    PARAM_DEFINE_FLOAT(FOLL_POS_P, MB_FOLLOW_POS_P_DEFAULT),

    // @Param: _OPTIONS
    // @DisplayName: Follow options
    // @Description: Follow options bitmask
    // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
    // @User: Standard
    PARAM_DEFINE_INT16(FOLL_OPTIONS, 0),
};

PARAM_GROUP_DEFINE(FOLL, var_info);

struct mb_follow follow;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/* 
   The constructor also initialises the proximity sensor. Note that this
   constructor is not called until detect() returns true, so we
   already know that we should setup the proximity sensor
*/
void mb_follow_ctor()
{
    p_ctrl_init(&follow._p_pos, MB_FOLLOW_POS_P_DEFAULT);
    jitter_correction(&follow._jitter, 3000, 100);

    mb_follow_assign_param();
}

// returns true if library is enabled
bool mb_follow_enabled() { return follow._enabled; }

// set which target to follow
void mb_follow_set_target_sysid(uint8_t sysid) { follow._sysid = sysid; }

// get target sysid
uint8_t mb_follow_get_target_sysid() { return follow._sysid; }

// get position controller.  this controller is not used within this library but it is convenient to hold it here
const P_ctrl *mb_follow_get_pos_p() { return &follow._p_pos; }

// restore offsets to zero if necessary, should be called when vehicle exits follow mode
void mb_follow_clear_offsets_if_required()
{
    if (follow._offsets_were_zero) {
        vec3_zero(&follow._offset);;
    }
    follow._offsets_were_zero = false;
}

// get target's estimated location
bool mb_follow_get_target_location_and_velocity(Location *loc, Vector3f_t *vel_ned)
{
    // exit immediately if not enabled
    if (!follow._enabled) {
        return false;
    }

    // check for timeout
    if ((follow._last_location_update_ms == 0) || (time_millis() - follow._last_location_update_ms > MB_FOLLOW_TIMEOUT_MS)) {
        return false;
    }

    // calculate time since last actual position update
    const float dt = (time_millis() - follow._last_location_update_ms) * 0.001f;

    // get velocity estimate
    if (!mb_follow_get_velocity_ned(vel_ned, dt)) {
        return false;
    }

    // project the vehicle position
    Location last_loc = follow._target_location;
    location_offset(&last_loc, vel_ned->x * dt, vel_ned->y * dt);
    last_loc.alt -= vel_ned->z * 100.0f * dt; // convert m/s to cm/s, multiply by dt.  minus because NED

    // return latest position estimate
    *loc = last_loc;
    return true;
}

// get velocity estimate in m/s in NED frame using dt since last update
bool mb_follow_get_velocity_ned(Vector3f_t *vel_ned, float dt)
{
    vel_ned->x = follow._target_velocity_ned.x + (follow._target_accel_ned.x * dt);
    vel_ned->y = follow._target_velocity_ned.y + (follow._target_accel_ned.y * dt);
    vel_ned->z = follow._target_velocity_ned.z + (follow._target_accel_ned.z * dt);

    return true;
}

// get distance vector to target (in meters) and target's velocity all in NED frame
bool mb_follow_get_target_dist_and_vel_ned(Vector3f_t *dist_ned, Vector3f_t *dist_with_offs, Vector3f_t *vel_ned)
{
    ahrs_view* ahrs = get_ahrs_view();

    // get our location
    Location current_loc;
    if (!ahrs_get_location(ahrs, &current_loc)) {
         mb_follow_clear_dist_and_bearing_to_target();
         return false;
    }

    // get target location and velocity
    Location target_loc;
    Vector3f_t veh_vel;
    if (!mb_follow_get_target_location_and_velocity(&target_loc, &veh_vel)) {
        mb_follow_clear_dist_and_bearing_to_target();
        return false;
    }

    // change to altitude above home if relative altitude is being used
    if (target_loc.relative_alt == 1) {
        current_loc.alt -= ahrs_get_home2(ahrs)->alt;
    }

    // calculate difference
    const Vector3f_t dist_vec = location_get_distance_ned(&current_loc, &target_loc);

    // fail if too far
    if (math_flt_positive(follow._dist_max) && (vec3_length(&dist_vec) > follow._dist_max)) {
        mb_follow_clear_dist_and_bearing_to_target();
        return false;
    }

    // initialise offsets from distance vector if required
    mb_follow_init_offsets_if_required(&dist_vec);

    // get offsets
    Vector3f_t offsets = VECTOR3F_DEFAULT_VALUE;
    if (!mb_follow_get_offsets_ned(&offsets)) {
        mb_follow_clear_dist_and_bearing_to_target();
        return false;
    }

    // calculate results
    *dist_ned = dist_vec;
    vec3_add(dist_with_offs, &dist_vec, &offsets);
    *vel_ned = veh_vel;

    // record distance and heading for reporting purposes
    if (math_flt_zero(dist_with_offs->x) && math_flt_zero(dist_with_offs->y)) {
        mb_follow_clear_dist_and_bearing_to_target();
    } else {
        follow._dist_to_target = math_sqrtf(sq(dist_with_offs->x) + sq(dist_with_offs->y));
        follow._bearing_to_target = degrees(atan2f(dist_with_offs->y, dist_with_offs->x));
    }

    return true;
}

// set recorded distance and bearing to target to zero
void mb_follow_clear_dist_and_bearing_to_target()
{
    follow._dist_to_target = 0.0f;
    follow._bearing_to_target = 0.0f;
}

// initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required
void mb_follow_init_offsets_if_required(const Vector3f_t *dist_vec_ned)
{
    // return immediately if offsets have already been set
    if (!vec3_is_zero(&follow._offset)) {
        return;
    }
    follow._offsets_were_zero = true;

    float target_heading_deg;
    if ((follow._offset_type == MB_FOLLOW_OFFSET_TYPE_RELATIVE) && mb_follow_get_target_heading_deg(&target_heading_deg)) {
        // rotate offsets from north facing to vehicle's perspective
        mb_follow_rotate_vector2(&follow._offset, -dist_vec_ned->x, -dist_vec_ned->y, -dist_vec_ned->z, -target_heading_deg);
        gcs_send_text(MAV_SEVERITY_INFO, "Relative follow offset loaded");
    } else {
        // initialise offset in NED frame
        follow._offset.x = -dist_vec_ned->x;
        follow._offset.y = -dist_vec_ned->y;
        follow._offset.z = -dist_vec_ned->z;

        // ensure offset_type used matches frame of offsets saved
        follow._offset_type = MB_FOLLOW_OFFSET_TYPE_NED;
        gcs_send_text(MAV_SEVERITY_INFO, "N-E-D follow offset loaded");
    }
}

// rotate 3D vector clockwise by specified angle (in degrees)
void mb_follow_rotate_vector(Vector3f_t *vec2,const Vector3f_t *vec, float angle_deg)
{
    // rotate roll, pitch input from north facing to vehicle's perspective
    const float cos_yaw = cosf(radians(angle_deg));
    const float sin_yaw = sinf(radians(angle_deg));

    vec2->x = (vec->x * cos_yaw) - (vec->y * sin_yaw);
    vec2->y = (vec->y * cos_yaw) + (vec->x * sin_yaw);
    vec2->z = vec->z;
}

// rotate 3D vector clockwise by specified angle (in degrees)
void mb_follow_rotate_vector2(Vector3f_t *vec2, const float x, const float y, const float z, float angle_deg)
{
    // rotate roll, pitch input from north facing to vehicle's perspective
    const float cos_yaw = cosf(radians(angle_deg));
    const float sin_yaw = sinf(radians(angle_deg));

    vec2->x = (x * cos_yaw) - (y * sin_yaw);
    vec2->y = (y * cos_yaw) + (x * sin_yaw);
    vec2->z = z;
}

// get offsets in meters in NED frame
bool mb_follow_get_offsets_ned(Vector3f_t *offset)
{
    const Vector3f_t *off = &follow._offset;

    // if offsets are zero or type is NED, simply return offset vector
    if (vec3_is_zero(off) || (follow._offset_type == MB_FOLLOW_OFFSET_TYPE_NED)) {
        *offset = (*off);
        return true;
    }

    // offset type is relative, exit if we cannot get vehicle's heading
    float target_heading_deg;
    if (!mb_follow_get_target_heading_deg(&target_heading_deg)) {
        return false;
    }

    // rotate offsets from vehicle's perspective to NED
    mb_follow_rotate_vector(offset, off, target_heading_deg);
    return true;
}

// get user defined yaw behaviour
enum FollowYawBehave mb_follow_get_yaw_behave() { return (enum FollowYawBehave)follow._yaw_behave; }

// get target's heading in degrees (0 = north, 90 = east)
bool mb_follow_get_target_heading_deg(float *heading)
{
    // exit immediately if not enabled
    if (!follow._enabled) {
        return false;
    }

    // check for timeout
    if ((follow._last_heading_update_ms == 0) || (time_millis() - follow._last_heading_update_ms > MB_FOLLOW_TIMEOUT_MS)) {
        return false;
    }

    // return latest heading estimate
    *heading = follow._target_heading;
    return true;
}

// get target's estimated location and velocity (in NED), with offsets added
bool mb_follow_get_target_location_and_velocity_ofs(Location *loc, Vector3f_t *vel_ned)
{
    Vector3f_t ofs;
    if (!mb_follow_get_offsets_ned(&ofs) ||
        !mb_follow_get_target_location_and_velocity(loc, vel_ned)) {
        return false;
    }
    // apply offsets
    location_offset(loc, ofs.x, ofs.y);
    loc->alt -= ofs.z*100;
    return true;
}

// return true if we have a target
bool mb_follow_have_target(void)
{
    if (!follow._enabled) {
        return false;
    }

    // check for timeout
    if ((follow._last_location_update_ms == 0) || (time_millis() - follow._last_location_update_ms > MB_FOLLOW_TIMEOUT_MS)) {
        return false;
    }
    return true;
}

// handle mavlink DISTANCE_SENSOR messages
void mb_follow_handle_msg(const mavlink_message_t *msg)
{
    // exit immediately if not enabled
    if (!follow._enabled) {
        return;
    }

    // skip our own messages
    if (msg->sysid == mavlink_system.sysid) {
        return;
    }

    // skip message if not from our target
    if (follow._sysid != 0 && msg->sysid != follow._sysid) {
        if (follow._automatic_sysid) {
            // maybe timeout who we were following...
            if ((follow._last_location_update_ms == 0) || (time_millis() - follow._last_location_update_ms > MB_FOLLOW_SYSID_TIMEOUT_MS)) {
                follow._sysid = 0;
            }
        }
        return;
    }

    // decode global-position-int message
    bool updated = false;

    switch (msg->msgid) {
    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
        // decode message
        mavlink_global_position_int_t packet;
        mavlink_msg_global_position_int_decode(msg, &packet);

        // ignore message if lat and lon are (exactly) zero
        if ((packet.lat == 0 && packet.lon == 0)) {
            return;
        }

        follow._target_location.lat = packet.lat;
        follow._target_location.lng = packet.lon;

        // select altitude source based on FOLL_ALT_TYPE param 
        if (follow._alt_type == MB_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
            // above home alt
            location_set_alt_cm(&follow._target_location, packet.relative_alt / 10, ALT_FRAME_ABOVE_HOME);
        } else {
            // absolute altitude
            location_set_alt_cm(&follow._target_location, packet.alt / 10, ALT_FRAME_ABSOLUTE);
        }

        follow._target_velocity_ned.x = packet.vx * 0.01f; // velocity north
        follow._target_velocity_ned.y = packet.vy * 0.01f; // velocity east
        follow._target_velocity_ned.z = packet.vz * 0.01f; // velocity down

        // get a local timestamp with correction for transport jitter
        follow._last_location_update_ms = jitter_correct_offboard_timestamp_msec(&follow._jitter, packet.time_boot_ms, time_millis());
        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
            follow._target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
            follow._last_heading_update_ms = follow._last_location_update_ms;
        }
        // initialise _sysid if zero to sender's id
        if (follow._sysid == 0) {
            follow._sysid = msg->sysid;
            follow._automatic_sysid = true;
        }
        updated = true;
        break;
    }
    case MAVLINK_MSG_ID_FOLLOW_TARGET: {
        // decode message
        mavlink_follow_target_t packet;
        mavlink_msg_follow_target_decode(msg, &packet);

        // ignore message if lat and lon are (exactly) zero
        if ((packet.lat == 0 && packet.lon == 0)) {
            return;
        }
        // require at least position
        if ((packet.est_capabilities & (1<<0)) == 0) {
            return;
        }

        Location new_loc = follow._target_location;
        new_loc.lat = packet.lat;
        new_loc.lng = packet.lon;
        location_set_alt_cm(&new_loc, packet.alt*100, ALT_FRAME_ABSOLUTE);

        // FOLLOW_TARGET is always AMSL, change the provided alt to
        // above home if we are configured for relative alt
        if (follow._alt_type == MB_FOLLOW_ALTITUDE_TYPE_RELATIVE &&
            !location_change_alt_frame(&new_loc, ALT_FRAME_ABOVE_HOME)) {
            return;
        }
        follow._target_location = new_loc;

        if (packet.est_capabilities & (1<<1)) {
            follow._target_velocity_ned.x = packet.vel[0]; // velocity north
            follow._target_velocity_ned.y = packet.vel[1]; // velocity east
            follow._target_velocity_ned.z = packet.vel[2]; // velocity down
        } else {
            vec3_zero(&follow._target_velocity_ned);
        }

        // get a local timestamp with correction for transport jitter
        follow._last_location_update_ms = jitter_correct_offboard_timestamp_msec(&follow._jitter, packet.timestamp, time_millis());

        if (packet.est_capabilities & (1<<3)) {
            Quat_t q = {packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
            float r, p, y;
            quat_to_euler2(&q, &r, &p, &y);
            follow._target_heading = degrees(y);
            follow._last_heading_update_ms = follow._last_location_update_ms;
        }

        // initialise _sysid if zero to sender's id
        if (follow._sysid == 0) {
            follow._sysid = msg->sysid;
            follow._automatic_sysid = true;
        }
        updated = true;
        break;
    }
    }

#if 0
    if (updated) {
        // get estimated location and velocity
        Location loc_estimate = {0};
        Vector3f_t vel_estimate;
        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));

        // log lead's estimated vs reported position
// @LoggerMessage: FOLL
// @Description: Follow library diagnostic data
// @Field: TimeUS: Time since system startup
// @Field: Lat: Target latitude
// @Field: Lon: Target longitude
// @Field: Alt: Target absolute altitude
// @Field: VelN: Target earth-frame velocity, North
// @Field: VelE: Target earth-frame velocity, East
// @Field: VelD: Target earth-frame velocity, Down
// @Field: LatE: Vehicle latitude
// @Field: LonE: Vehicle longitude
// @Field: AltE: Vehicle absolute altitude
        AP::logger().WriteStreaming("FOLL",
                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
                                               "sDUmnnnDUm",    // units
                                               "F--B000--B",    // mults
                                               "QLLifffLLi",    // fmt
                                               AP_HAL::micros64(),
                                               _target_location.lat,
                                               _target_location.lng,
                                               _target_location.alt,
                                               (double)_target_velocity_ned.x,
                                               (double)_target_velocity_ned.y,
                                               (double)_target_velocity_ned.z,
                                               loc_estimate.lat,
                                               loc_estimate.lng,
                                               loc_estimate.alt
                                               );
    }
#endif
}

// get horizontal distance to target (including offset) in meters (for reporting purposes)
float mb_follow_get_distance_to_target() { return follow._dist_to_target; }

// get bearing to target (including offset) in degrees (for reporting purposes)
float mb_follow_get_bearing_to_target() { return follow._bearing_to_target; }

// get system time of last position update
uint32_t mb_follow_get_last_update_ms() { return follow._last_location_update_ms; }

// returns true if a follow option enabled
bool mb_follow_option_is_enabled(enum FollowOption option) { return (follow._options & (uint16_t)option) != 0; }

static void mb_follow_assign_param()
{
    param_link_variable(PARAM_ID(FOLL, FOLL_ENABLE), &follow._enabled);
    param_link_variable(PARAM_ID(FOLL, FOLL_SYSID), &follow._sysid);
    param_link_variable(PARAM_ID(FOLL, FOLL_DIST_MAX), &follow._dist_max);
    param_link_variable(PARAM_ID(FOLL, FOLL_OFS_TYPE), &follow._offset_type);
    param_link_variable(PARAM_ID(FOLL, FOLL_OFS_X), &follow._offset.x);
    param_link_variable(PARAM_ID(FOLL, FOLL_OFS_Y), &follow._offset.y);
    param_link_variable(PARAM_ID(FOLL, FOLL_OFS_Z), &follow._offset.z);
#if !(MB_BUILD_TYPE(MICROBEE_BUILD_Rover))
    param_link_variable(PARAM_ID(FOLL, FOLL_YAW_BEHAVE), &follow._yaw_behave);
    param_link_variable(PARAM_ID(FOLL, FOLL_ALT_TYPE), &follow._alt_type);
#endif
    param_link_variable(PARAM_ID(FOLL, FOLL_POS_P), &follow._p_pos._kp);
    param_link_variable(PARAM_ID(FOLL, FOLL_OPTIONS), &follow._options);
}
/*------------------------------------test------------------------------------*/


